
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Land.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "Land.h"

/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void Land_Init(Land* pLand)
{
    pLand->landFlg = true;
    pLand->takeoffFlg = false;
    vec3_zero(&(pLand->force));
    vec3_zero(&(pLand->moment));
}


void Land_Run(Land* pLand, Vector3f_t* ve, Vector3f_t* forec, Vector3f_t* wb, float alt, Quat_t* quat, float mg)
{
    //根据据地面高度 设置落地起飞标志
    if ((alt > 0.3f) && (ve->vec3[2] < -0.1f) && !pLand->takeoffFlg)
    {
        pLand->takeoffFlg = true;
        pLand->landFlg = false;
    }

    if ((alt < -0.05f) && (ve->vec3[2] > 0.1f) && !pLand->landFlg)
    {
        pLand->takeoffFlg = false;
        pLand->landFlg = true;
    }

    vec3_zero(&(pLand->force));
    vec3_zero(&(pLand->moment));
    //计算反向力
    //计算反向力
    if (math_flt_zero(alt) && (forec->vec3[2] > 0))
    {
        //地面起飞
        pLand->force.vec3[2] = -forec->vec3[2];
    }
    else if (alt < -0.001f)
    {
        if (alt > -0.05f)
        {
            if (ve->vec3[2] > 0)
            {
                pLand->force.vec3[2] = -forec->vec3[2] + 20.0f * alt - 150.0f * ve->vec3[2];
            }
            else
            {
                pLand->force.vec3[2] = -forec->vec3[2] + 20.0f * alt - 15.0f * ve->vec3[2];
            }
        }
        else if (alt > -0.1f)
        {
            pLand->force.vec3[2] = -mg + 20.0f * alt - 200.0f * ve->vec3[2];
        }
        else
        {
            pLand->force.vec3[2] = -mg + 20.0f * alt - 300.0f * ve->vec3[2];
        }

        //pLand->force.vec3[2] = -forec->vec3[2];
        pLand->force.vec3[0] = -10 * ve->vec3[0];
        pLand->force.vec3[1] = -10 * ve->vec3[1];

        //计算力矩 根据倾斜程度
        Quat_t quat_yaw, quet;
        float yaw = quat_get_yaw(quat);
        quat_from_euler(&quat_yaw, 0.0f, 0.0f, yaw);

        //计算倾斜四元数
        quat_invert(&quat_yaw);
        quet = quat_mult(&quat_yaw, quat);

        Vector3f_t M1;
        vec3_set(&M1, quet.x, quet.y, quet.z);
        vec3_mult(&M1, &M1, -20.0f);

        Vector3f_t tGyro_m;
        vec3_mult(&tGyro_m, wb, -5.0f);
        vec3_add(&M1, &M1, &tGyro_m);

        float param;
        param = mg * 0.3333f / GRAVITY_MSS;
        vec3_mult(&(pLand->moment), &M1, param);


        //Vec3_Zero(&(pLand->moment));
    }


}

bool Land_GetLandFlg(Land* pLand)
{
    return pLand->landFlg;
}

bool Land_GetTakeOffFlg(Land* pLand)
{
    return pLand->takeoffFlg;
}
Vector3f_t* Land_GetForce(Land* Land)
{
    return &(Land->force);
}
Vector3f_t* Land_GetMoment(Land* Land)
{
    return &(Land->moment);
}

/*------------------------------------test------------------------------------*/

